/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include <algorithm>
#include <stdio.h>
#include <sys/stat.h>
#include "Camera.h"
#include "owndebug.h"
#include "World.h"
#include "GLProvider.h"
#include "mathutils.h"


#define gle dPrint(1,"GLError:%s",gluErrorString(glGetError()));

#define PRINT_TIME dPrint(1,"%d",clock());

bool CCamera::RenderingForCamera=false;

CCamera::CCamera(CWorld *World, CObject *ParentObject, dBodyID parentBody,
                dReal *posOffset, dMatrix3 rotOffset, unsigned int resoX,
                unsigned int resoY, unsigned int fovy, gimi::GIMI* gimi,
                MaCI::MaCICtrl::CMaCICtrlServer* mcs,
                std::string cameraInterfaceName, 
                std::string PTUInterfaceName, bool noiseMode)//,
                //bool stereo, float distance)
:CObject(World,ParentObject),
PosOffset(posOffset),
resoX(resoX),
resoY(resoY),
fovy(fovy),
fps(15),
timer(0),
gimi(gimi),
mcs(mcs),
jointServer(mcs,-1),
imageServer(mcs,-1),
pan(0),
tilt(0),
//stereo(stereo),
//distance(distance),
noiseMode(noiseMode),
glInited(false),
glOK(false),
contextID(0),
errorImageLoaded(false),
errorImageJPGData(NULL),
errorDataSize(0)
{
  for (int i=0; i<12;i++)
    this->RotOffset[i]=rotOffset[i];

  Parts.push_back(box=new CBox(0.05,0.05,0.10,this,SpaceID,parentBody));

  dGeomSetOffsetPosition(box->GeomID,posOffset[0],posOffset[1],posOffset[2]);
  dGeomSetOffsetRotation(box->GeomID,rotOffset);

  rgbData=new unsigned char[resoX*resoY*3];
  rgbDataFlipped=new unsigned char[resoX*resoY*3];

  jointServer.SetInterfaceInstanceName(PTUInterfaceName);
  imageServer.SetInterfaceInstanceName(cameraInterfaceName);
  MaCI::JointGroupCtrl::TEventTypes eventTypes;
  eventTypes.SetEventType(MaCI::JointGroupCtrl::KTypeJointPosition); 
  jointServer.SetEventTypes(eventTypes);
  setPanAndTilt();

}

CCamera::~CCamera()
{
  delete []rgbData;
  delete []rgbDataFlipped;
}

void CCamera::Activate()
{
  initGimi();
}


void CCamera::Deactivate()
{
  imageServer.Close();
  jointServer.Close();
}


void CCamera::Update(float StepSize)
{
  
  timer=timer+StepSize;
  MaCI::JointGroupCtrl::CJointGroupCtrlData jointData;
  MaCI::JointGroupCtrl::TReplyInfo replyInfo;
  if (jointServer.WaitCommand(jointData,replyInfo,0)==MaCI::KMaCIOK)
  {
    if (jointData.GetCommand()->cmd==MaCI::JointGroupCtrl::KCommandSetGroupPositions)
    {
      MaCI::JointGroupCtrl::TJointPositionPtrArray array = jointData.GetJointPositionPtrArray();
      for(EACH_IN_i(array)){
        if ((*i)->jointNumber==0)
          pan=(*i)->position;
        if ((*i)->jointNumber==1)
          tilt=(*i)->position;
      }
      
      setPanAndTilt();
      //Then send an event when joints are moved..
      
      MaCI::JointGroupCtrl::CJointGroupCtrlData eventData;
      eventData.CreateInternalBinBag();
      eventData.SetCommand(MaCI::JointGroupCtrl::TCommand(MaCI::JointGroupCtrl::KCommandEvent));
      eventData.SetPosition(MaCI::JointGroupCtrl::TJointPosition(pan,0));
      eventData.SetPosition(MaCI::JointGroupCtrl::TJointPosition(tilt,1));
      //      eventData.Print(1);
      jointServer.SendEvent(eventData);

    } else if (jointData.GetCommand()->cmd==MaCI::JointGroupCtrl::KCommandUpdateState)
    {
      MaCI::JointGroupCtrl::CJointGroupCtrlData eventData;
      eventData.CreateInternalBinBag();
      eventData.SetCommand(MaCI::JointGroupCtrl::TCommand(MaCI::JointGroupCtrl::KCommandEvent));
      eventData.SetPosition(MaCI::JointGroupCtrl::TJointPosition(pan,0));
      eventData.SetPosition(MaCI::JointGroupCtrl::TJointPosition(tilt,1));
      //      eventData.Print(1);
      jointServer.SendEvent(eventData);
    } else if (jointData.GetCommand()->cmd==MaCI::JointGroupCtrl::KCommandGetJointGroupStatus)
    {
      MaCI::JointGroupCtrl::CJointGroupCtrlData replyJointData;
      replyJointData.EncodeTo(&binbag);
      replyJointData.SetCommand(MaCI::JointGroupCtrl::TCommand(
                              MaCI::JointGroupCtrl::KReplyGetJointGroupStatus));
      replyJointData.SetJointGroupStatus(MaCI::JointGroupCtrl::TJointGroupStatus(MaCI::JointGroupCtrl::KJointGroupStatusOpen));
      
      
      jointServer.SendReply(replyJointData,replyInfo);
    }
  }
  
}

void CCamera::Draw()
{
//#ifdef DISABLE_CAMERA

  if (!glInited && !noiseMode)
    initGL();
  
  if ((!glOK && glInited) || noiseMode)
  {
  
    if ( timer>=1.0/fps && (gimi->getSubscriberCount(GIMI_PROTOCOL_MACI_IMAGE,minorType)>0))
    {
      //generate noise
      for (unsigned int i=0; i< resoX*resoY; i++)
      {
        char val=rand()%255;
        rgbData[i*3]=val;
        rgbData[i*3+1]=val;
        rgbData[i*3+2]=val;
      }
      //send the noise
      //dPrint(1,"sending noise ffs");
      sendImage(rgbData,resoX*resoY*3);
      
      timer=0;
    }
  } else {
  
    
  //#else //DISABLE_CAMERA
    
    
    //PRINT_TIME
    if (timer>=1.0/fps && !RenderingForCamera )
    {
      
      
      
      
      //dPrint(1,"subs=%d",gimi->getSubscriberCount(GIMI_PROTOCOL_JPEG_WITH_TIMESTAMP,minorType));
      timer=0;
      
      RenderingForCamera=true;
  
      SimWorld->GetGLProvider()->SetContext(contextID);
      //PRINT_TIME
      
      const dReal *R;
      const dReal *P;
      R=dGeomGetRotation(box->GeomID);
      P=dGeomGetPosition(box->GeomID);
  
      //float offset[3]={distance/2.0,0,0}; //offset between cameras
      //float offsetres[3];
      float lookdir[3]={0,0,1};
      float updir[3]={-1,0,0};
      float lookres[3];
      float upres[3];
  
      RotateVector(R,lookdir,lookres);
      RotateVector(R,updir,upres);
      //RotateVector(R,offset,offsetres);
  
      glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
  
  
      glPushMatrix();
        glMatrixMode(GL_PROJECTION);
        glLoadIdentity();
        gluPerspective(fovy,1.0*resoX/resoY,0.1,1000);
        glMatrixMode(GL_MODELVIEW);
        glLoadIdentity();
        gluLookAt(P[0],P[1],P[2],P[0]+lookres[0],P[1]+lookres[1],P[2]+lookres[2],
                  upres[0],upres[1],upres[2]);
  
        //dPrint(1,"Drawing the world for camera");
        SimWorld->Draw();
  
      glPopMatrix();
      
      glFlush();
  
      //PRINT_TIME
      
      if (gimi->getSubscriberCount(GIMI_PROTOCOL_MACI_IMAGE,minorType)>0)
      {
      
        glReadBuffer(GL_FRONT_LEFT);
        glReadPixels(0,0,resoX,resoY,GL_RGB,GL_UNSIGNED_BYTE,rgbData);
    
        //PRINT_TIME
        
        //flip image data
        for (unsigned int i=0; i< resoY;i++)
        {
          memcpy((void*)(rgbDataFlipped+(resoY-i-1)*resoX*3),(void*)(rgbData+i*resoX*3),resoX*3);
        }
    
        sendImage(rgbDataFlipped,resoX*resoY*3);
      }
      //PRINT_TIME
      
      glutSwapBuffers();
      
      //switch back to gui's drawable
      SimWorld->GetGLProvider()->ResetContext();
  
      RenderingForCamera=false;
      //return;
    } 
  //#endif //DISABLE_CAMERA
  }
  
  for (ObjectsIter i=Children.begin();i!=Children.end();i++)
  {
    (*i)->Draw();
  }

  for (PrimitivesIter i=Parts.begin();i!=Parts.end();i++)
  {
    (*i)->Draw();
  }

  /*
  dReal M[16];
  ODEToOGL(dGeomGetRotation(box->GeomID), dGeomGetPosition(box->GeomID), M);
  glPushMatrix();
  glMultMatrixf(M);
  glBegin(GL_LINES);
    glColor3f(1,0,0);
    glVertex3f(0,0,0);
    glVertex3f(0,0,1);
  glEnd();
  glPopMatrix();
  */
  
}

void CCamera::OnCollision(CPrimitive *primitive, int numc, dContact *contacts, CPrimitive *primitive2)
{

}

void CCamera::initGL()
{
  dPrint(1,"initing gl");
  glInited=true;
  CGLProvider *glp=SimWorld->GetGLProvider();
  
  if (glp)
  {
    if ((contextID=glp->CreateNewContext(resoX, resoY, "Camera")))
      glOK=true;
    
    SimWorld->InitGraphics();
    
  }
}

void CCamera::initGimi()
{
  //dPrint(1,"init gimi");
  MaCI::JointGroupCtrl::TJointInformationArray a;
  MaCI::JointGroupCtrl::TJointInformation_Payload panInfo(0,
      MaCI::JointGroupCtrl::KJointControlAngular, 0, 0, 1, -M_PI/2.0, 
      M_PI/2.0);
  panInfo.SetName("Pan");
  MaCI::JointGroupCtrl::TJointInformation_Payload tiltInfo(1,
      MaCI::JointGroupCtrl::KJointControlAngular, 0, 0, 1, -M_PI/2.0, 
      M_PI/2.0);
  tiltInfo.SetName("Tilt");
  a.push_back(panInfo);
  a.push_back(tiltInfo);
  jointServer.SetJointGroupInfo(a);
  
  
  MaCI::EMaCIError e=jointServer.Open();
  if (e!=MaCI::KMaCIOK)
  {
    dPrint(1,"Couldnt open JointGroupCtrlServer");
  } else {
    dPrint(1,"JointGroupCtrlServer opened");
  }
  
  imageData.CreateInternalBinBag();
    
  e=imageServer.Open();
  if (e!=MaCI::KMaCIOK)
  {
    dPrint(1,"Couldnt open ImageServer");
  } else {
    dPrint(1,"ImageServer opened");
  }
  
}


void CCamera::setPanAndTilt()//float pan, float tilt)
{
  dMatrix3 R,r;
  
  //dRFromEulerAngles(R,DegToRad(90),pan,tilt);
  
  //dRSetIdentity(R);
  dRFromEulerAngles(R,DegToRad(90),pan,tilt);
  
  OdeMxM3(R, RotOffset, r);
  
  //dRFromEulerAngles(R,0,pan+DegToRad(40),tilt+DegToRad(180));
  
  dGeomSetOffsetRotation(box->GeomID,r);
  //dGeomSetRotation(box->GeomID,R);
}

void CCamera::sendImage(unsigned char *data, int size)
{
  MaCI::Image::CImageContainer container;
  
  container.SetImageData(data,size,
                          MaCI::Image::KImageDataRGB,resoX,resoY,MaCI::Image::KImageDataPNG);
  imageData.AddImage(0,container);
  imageServer.SendImageData(imageData);
  imageData.Reset();

}

void CCamera::sendErrorImage()
{
  if (!errorImageLoaded)
  {
    struct stat fileStat;
    FILE *file=fopen("camera_error.jpg","rb");
    
    if (file)
    {
      stat("camera_error.jpg",&fileStat);
      errorDataSize=fileStat.st_size;
      errorImageJPGData=new unsigned char[errorDataSize];
      fread(errorImageJPGData,1,errorDataSize,file);
      errorImageLoaded=true;
      //these are known
      resoX=320;
      resoY=240;
      fclose(file);
    } else {
      
    }
    
  }/* else {
    sendImage(errorImageJPGData,errorDataSize,MaCI::Image::KImageDataJPEG);
  }*/
  
  //send image
  
}
